Unicycle Model

• The unicycle model represents a mobile robot on a plane.
• Typical example: differential robot with two lateral wheels.
• Nonholonomic constraint: movement is only allowed in the direction of the orientation.

State Coordinates and Control Variables

When trying to estimate the position of a moving target, the problems become more complex. An additional term should be considered in the model, i.e., the orientation of the target. The new coordinates are \(\mathbf{q} = [x, y, \theta]^T\), where:

• \(x, y\) are the position of the target.
• \(\theta\) is the orientation of the target.

The control variable of the system are:

• \(v\) the velocity of the target.
• \(\omega\) the angular velocity of the target.

The dynamics of the system can be given by:

\[ \begin{aligned} \dot{x} &= v \cos(\theta), \\ \dot{y} &= v \sin(\theta), \\ \dot{\theta} &= \omega. \end{aligned} \]

Discretizing the Model

To simulate numerically, we discretize with time step \(\Delta t\):

\[ \begin{cases} x_{k+1}=x_k+v_k\cos\theta_k\,\Delta t \\ y_{k+1}=y_k+v_k\sin\theta_k\,\Delta t \\ \theta_{k+1}=\theta_k+\omega_k\,\Delta t \end{cases} \]

Self-localization problem

Localization of a master using only ranging information from UWB anchors.

What are the information available?

• Distance between the master and the anchors
• Distance between the anchors

Given the points \[ \mathbf{N} = \begin{bmatrix} \mathbf{N}_0 & \cdots & \mathbf{N}_n \end{bmatrix} = \begin{bmatrix} x_0 & \cdots & x_n \\ y_0 & \cdots & y_n \end{bmatrix}, \]

where \(\mathbf{N}_i\) is the position of the \(i\)-th anchor. Let us assume that the \(i\)-th node has access to the distances \[ \rho_{i,j} = \|\mathbf{N}_i - \mathbf{N}_j\| = \sqrt{(x_i - x_j)^2 + (y_i - y_j)^2}, \] so that the symmetric squared Euclidean distance matrix \[ \mathbf{D} = \begin{bmatrix} 0 & \rho^2_{0,1} & \cdots & \rho^2_{0,n} \\ \rho^2_{1,0} & 0 & \cdots & \rho^2_{1,n} \\ \vdots & \vdots & \ddots & \vdots \\ \rho^2_{n,0} & \rho^2_{n,1} & \cdots & 0 \end{bmatrix}, \] can be built. This is the matrix that we have access to.

How do we extract the points form the distances?

Using the double centering matrix \[ \mathbf{H} = \mathbf{I}_{n+1} - \frac{1}{n+1} \mathbf{e}\mathbf{e}^T, \] where \(\mathbf{e}\mathbf{e}^T = \mathbf{1}_{n+1} \mathbf{1}_{n+1}^T\), \(\mathbf{1}_{n+1}\) is a column vector filled with \(n+1\) ones and \(\mathbf{I}_{n+1}\) is the identity matrix of dimension \(n+1 \times n+1\), to transform \(\mathbf{D}\), we obtain the Gram matrix

\[ \mathbf{G} = -\frac{1}{2} \mathbf{H}\mathbf{D}\mathbf{H}. \]

This transformation turns pairwise Euclidean distances into pairwise inner products of vectors.

Let us define \(\mathbf{P} = \begin{bmatrix} \mathbf{p}_0 & \mathbf{p}_1 & \cdots & \mathbf{p}_n \end{bmatrix}^T\) as the matrix of node coordinates that generates the symmetric Euclidean matrix \(\mathbf{D}\), and that is a replica of \(\mathbf{N}\) but affected by geometric ambiguities.

Gram Matrix and Its Properties

The Gram matrix \(\mathbf{G}\) has a special structure: if \(\mathbf{G}\) is positive semi-definite, it can be decomposed into the product of coordinates (or vectors) that represent the points. Specifically, if \(\mathbf{G} = \mathbf{P} \mathbf{P}^T\), where \(\mathbf{P}\) contains the coordinates of the points we’re trying to find, then \(\mathbf{G}\) preserves the pairwise distances in the following sense:

\[ \mathbf{G}_{ij} = \langle \mathbf{p}_i, \mathbf{p}_j \rangle = \mathbf{p}_i^T \mathbf{p}_j, \]

This means that each entry \(\mathbf{G}_{ij}\) in the Gram matrix corresponds to the inner product of the coordinates of points \(\mathbf{p}_i\) and \(\mathbf{p}_j\). Since inner products determine the relative geometry of points, reconstructing \(\mathbf{P}\) will preserve these distances up to an affine transformation.

Eigen-Decomposition for Coordinate Recovery

To derive \(\mathbf{P}\), the following optimization problem needs to be solved: \[ \arg \min_{\mathbf{P}} \| \mathbf{G} - \mathbf{P} \mathbf{P}^T \|^2. \] The solution to this optimization problem is given by the eigen-decomposition, i.e. \[ \mathbf{P} = \begin{bmatrix} \mathbf{p}_0 & \cdots & \mathbf{p}_n \end{bmatrix} = \begin{bmatrix} \tilde{x}_0 & \cdots & \tilde{x}_n \\ \tilde{y}_0 & \cdots & \tilde{y}_n \end{bmatrix} = \mathbf{U} \sqrt{\mathbf{V}}, \] where \(\mathbf{V}\) is the diagonal matrix of the eigenvalues, and \(\mathbf{U}\) is the eigenvector matrix of \(\mathbf{G}\).

Specifically:

• \(\mathbf{G} = \mathbf{U} \mathbf{V} \mathbf{U}^T,\) where \(\mathbf{U}\) is the matrix of eigenvectors and \(\mathbf{V}\) is the diagonal matrix of eigenvalues.

• By taking \(\mathbf{P} = \mathbf{U} \sqrt{\mathbf{V}},\) we construct coordinates that satisfy \(\mathbf{G} \approx \mathbf{P} \mathbf{P}^T,\) meaning that \(\mathbf{P}\) will reproduce the distances in \(\mathbf{D}\) up to rotations, reflections, or translations.

Geometric Ambiguity

The resulting points \(\mathbf{P}\) are not exactly the same as the original points \(\mathbf{N}\); they are affine transformations of \(\mathbf{N}\), which means that they could be rotated, reflected, or translated versions of \(\mathbf{N}\). This is because the Gram matrix \(\mathbf{G}\) preserves distances, but it does not preserve absolute positioning or orientation in space. However, any two point sets \(\mathbf{N}\) and \(\mathbf{P}\) that produce the same \(\mathbf{G}\) will have identical pairwise distances, which is the key requirement for applications relying on \(\mathbf{D}\).

More precisely, if there exists an angle \(\theta \neq 2k\pi\) with \(k \in \mathbb{N}\) such that \[ \mathbf{N} = \begin{bmatrix} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{bmatrix} \mathbf{P} = \mathbf{R}(\theta) \mathbf{P}, \] then a rotation ambiguity occurs. The flipping problem takes place if \[ \mathbf{N} = \pm \begin{bmatrix} -1 & 0 \\ 0 & 1 \end{bmatrix} \mathbf{P} = \pm \mathbf{S} \mathbf{P}. \]

Example

Return to the self-localization problem

Until now we understood that given the distances is possible to reconstruct the position of the anchors up to a rotation, reflection, or translation. So we want to know if taking more measurements will help to find the relative position of the master with respect to the anchors.

Illustration of the problem

Position of the master and two anchors at time \(t_1\).

Illustration of the problem

Position of the master and two anchors at time \(t_2\).

Illustration of the problem

Position of the master and two anchors at time \(t_3\).

Moving Node Setup

Consider three time instants \(k\), \(k + 1\), and \(k + 2\) where the moving node \(\mathbf{N}_0\):

• Starts at position \(\mathbf{N}^k_0\),
• Moves to \(\mathbf{N}^{k+1}_0 = \mathbf{N}^k_0 + \mathbf{t}_k\), and
• Moves to \(\mathbf{N}^{k+2}_0 = \mathbf{N}^{k+1}_0 + \mathbf{t}_{k+1}\).

Here, \(\mathbf{t}_k = \begin{bmatrix} \Delta x_k \\ \Delta y_k \end{bmatrix}\) and \(\mathbf{t}_{k+1} = \begin{bmatrix} \Delta x_{k+1} \\ \Delta y_{k+1} \end{bmatrix}\) are translation vectors.

Distance Matrix Construction

With measurements \(\rho_{i,j} + \eta_{i,j}\) for three consecutive time instants:

• We build distance matrices \(\mathbf{D}^k\), \(\mathbf{D}^{k+1}\), and \(\mathbf{D}^{k+2}\).

• Use these distance matrices to compute estimated positions \(\hat{\mathbf{P}}^k\), \(\hat{\mathbf{P}}^{k+1}\), and \(\hat{\mathbf{P}}^{k+2}\) by solving the optimization problem: \[ \arg \min_{\mathbf{P}} \| \mathbf{G} - \mathbf{P} \mathbf{P}^T \|^2 \]

Alignment with Roto-Translation

1. Centering:
   • First, center \(\hat{\mathbf{P}}^k\) on the moving node \(0\): \[ \hat{\mathbf{P}}^k = \hat{\mathbf{P}}^k - \hat{\mathbf{p}}_{0,k} \]
2. Roto-Translation Alignment: Align \(\hat{\mathbf{P}}^{k+1}\) with \(\hat{\mathbf{P}}^k\) by solving: \[ \arg \min_{\theta, \mathbf{T}} \| \hat{\mathbf{P}}^k - \left(\mathbf{R}(\theta) \alpha \mathbf{S} \hat{\mathbf{P}}^{k+1} + \mathbf{T}\right) \| \] Here, \(\mathbf{R}(\theta)\) represents rotation by \(\theta\), and \(\mathbf{T}\) is the translation vector.

Path Estimation

• After alignment, the estimated translation \(\mathbf{T}\) provides an approximation of the actual displacement \(\mathbf{t}_k\) in the opposite direction.

• This alignment can be extended from \(\hat{\mathbf{P}}^k\) and \(\hat{\mathbf{P}}^{k+1}\) to the next time step \(\hat{\mathbf{P}}^{k+2}\), yielding the path of the moving node \(\mathbf{N}_0\) in its relative frame centered on \(\hat{\mathbf{p}}_{0,k}\).

Ambiguity in Flipping

• Although alignment provides the positions relative to \(\mathbf{N}_0\), it does not fully solve the flipping problem modeled by \(\alpha \mathbf{S}\).
• Due to the ambiguity, the points may appear as reflections across either the x-axis or y-axis.

Theorem: Flipping Ambiguity

Theorem: Given a set of \(m > 0\) node \(0\) motions, \[ \mathbf{N}_0^{k+q} = \mathbf{N}_0^{k+q-1} + \mathbf{t}_{k+q-1}, \quad q = 1, \ldots, m \] it is impossible to determine \(\alpha \mathbf{S}\) (flipping operation) if we have no additional knowledge about \(\mathbf{t}_{k+q-1}\).

Resolving Flipping with Rotation Direction

Corollary: - By knowing the sign of angle \(\beta = \arctan \frac{\Delta y_{k+1} - \Delta y_k}{\Delta x_{k+1} - \Delta x_k}\), we can resolve flipping. - The angle \(\beta\) gives the relative rotation direction of \(t_k\) to \(t_{k+1}\), indicating whether the movement is clockwise or counterclockwise.

Flipping Resolution: Intuition

• If the moving node \(0\) has rotated in a specific direction (e.g., counterclockwise), a flipped solution would imply a contradictory movement (e.g., clockwise).
• By verifying the sign of \(\beta\), we can remove incorrect solutions that do not match the actual direction of movement.

Conclusion

• This method allows localization of the moving node \(N_0\) within a relative frame centered on its initial estimated position.
• By incorporating direction of movement, we can resolve the remaining flipping ambiguity.